\chapter{算法原理}
\label{cha:principle}
\section{算法原理}
\label{sec:history}
Boundle Adjustment翻译过来是光束平差法，本质就是通过将其他视图的路标点通过重投影的方式投影到参考视图中，通过最小化重投影误差获得对位姿、坐标、相机参数的最大后验估计。
\section{算法理论分析}
\subsection{数学建模}

我们将问题进行数学上的描述。


现在定义在t=0...,...N时间内，机器人的位姿为:

\begin{equation}
  \begin{split}
    \label{equ:Xdef}
    x_{0},...,...,x_{N}\\x_{i}=(R_{i},t_{i})
  \end{split}
\end{equation}

同时有三维路标点(landmark)：
\begin{equation}
  \begin{split}
    \label{equ:=Ydef}
    y_{1},....,y_{M}
  \end{split}
\end{equation}

我们可以给出运动与观测的方程：


\begin{equation}
  \label{euq:observe_motion}
  \left\{
  \begin{aligned}
    x_{k}= f(x_{k-1},u)+w_{k} \\
    z_{k,j}=h(y_{j},w_{k})+v_{k,j}
  \end{aligned}
  \right.
\end{equation}

对于第一个公式：$f$ 为非线性函数，$u_{k}$为运动输入量，$w_{k}$为噪声项。

对于第二个公式：在k时刻对j时刻的路标点j进行观测，在图像上得到观测点$z_{k,j}$这是一个二维坐标点），h为非线性函数。

两个方程联合起来表示为：任意时刻我们拿到了z与u，通过z与u估计$x_{0},...,x_{N}$与$y_{1},....,y_{M}$服从什么类型的分布。

为了便于表达，我们将位姿与路标点合并写为：

\begin{equation}
  \begin{split}
    \label{equ:XYdef}
    x_{k}\triangleq {x_{k},y_{1},...,y_{M}}
  \end{split}
\end{equation}

观测量统一写为$z_{k}$

方程简化为：

\begin{equation}
  \label{euq:observe_motion}
  \left\{
  \begin{aligned}
    x_{k}= f(x_{k-1},u_{k})+w_{k} \\
    z_{k}=h(x_{k})+v_{k}
  \end{aligned}
  \right.
\end{equation}

Boundle Adjustment实际上是在解决一个状态估计问题。根据过去0到k时刻的数据，我们可以估计当前状态：

\begin{equation}
  \begin{split}
    \label{equ:Xdef}
    P(x_{k}|x_{0},u_{1:k},z_{1:k})
  \end{split}
\end{equation}


这是一个后验概率，对上式进行Bayes展开：

\begin{equation}
  P(x_{k}|x_{0},u_{1:k},z_{1:k})\propto P(z_{k}|x_{k})P(x_{k}|x_{0},u_{1:k},z_{1:k-1})
\end{equation}

$P(z_{k}|x_{k})$为似然概率,$P(x_{k}|x_{0},u_{1:k},z_{1:k-1})$为先验概率。我们直接求解后验分布是困难的，但是求一个状态最优估计，使得在该状态下，后验概率最大化(MAP)，是可行的。

\begin{equation}
  x_{MAP}^{*}=arg maxP(x|z)=arg maxP(z|x)P(x)
\end{equation}

通过Bayes法则，我们发现，求解最大后验概率相当于最大化似然和先验的乘积。而先验部分于x实际上是没有关系的。因此我们可以直接求解x的最大似然估计(MLE):

\begin{equation}
  x_{MAP}^{*}=arg maxP(z|x)
\end{equation}

此时，我们带入观测模型，对于某一次观测：

\begin{equation}
  z_{k,j}=h(y_{j},x_{k})+v_{k,j}
\end{equation}

我们假设噪声项$v_{k}~N(0,Q_{k,j})$,则观测数据的条件概率变为：

\begin{equation}
  P(z_{j,k}|x_{k},y_{j})=N(h(y_{j},x_{k}),Q_{k,j})
\end{equation}

这仍然是一个高斯分布，为了计算使它最大化的$y_{i}$,$x_{k}$，我们使用最小化负对数的方式。高斯分布的概率密度函数展开形式为：

\begin{equation}
\label{equ:Gauss}
  P(x)=\frac{1}{\sqrt{(2\pi)^{N}}det(\Sigma)}exp(-\frac{1}{2}(x-u))
\end{equation}

如公式\ref{equ:Gauss},我们取其负对数得到：

\begin{equation}
\label{equ:GaussNLog}
  -ln(P(x))=\frac{1}{2}ln((2\pi)^{N}det(\Sigma))+\frac{1}{2}(x-\mu)^{T}\Sigma^{-1}(x-\mu)
\end{equation}

对原分布求最大化相当于对负对数求最小化。在最小化公式\ref{equ:GaussNLog}的x时，第一项与x无关，可以略去，于是只要最小化右侧的二次型项，
就得到了对状态的最大似然估计。带入SLAM的观测模型，相当于我们在求：

\begin{equation}
  \label{equ:x*}
  x^{*}=arg min((z_{k,j}-h(x_{k},y_{j}))^{T}Q_{k,j}^{-1}(z_{k,j}-h(x_{k},y_{j})))
\end{equation}

我们发现，公式\ref{equ:x*}等价于最小化噪声项（即误差）的平方（$\Sigma$范数意义下）。因此，对于所有运动和任意观测，我们定义数据与
估计值之间的误差：

\begin{equation}
  \label{equ:error}
  \begin{split}
    e_{v,k}=x_{k}-f(x_{k-1},u_{k})\\e_{y,j,k}=z_{k,j}-h(x_{k},y_{j})
  \end{split}
\end{equation}

对公式\ref{equ:error}求平方和得到：

\begin{equation}
  \label{equ:LSP}
  J(x)=\sum_{k}e_{v,k}^{T}R_{k}^{-1}e_{v,k}+\sum_{k}\sum_{j}e_{y,k,j}^{T}Q_{k,j}^{-1}e_{y,j,k}
\end{equation}

这样我们得到了一个总体意义下的最小二乘问题。我们明白他的最优解等价于状态的最大似然估计。我们对该公式进行非线性优化即可获得一个较优解。

\subsection{问题回归}

我们得到了一个关于位姿估计的数学模型，现在我们需要根据具体的问题将优化函数表达出来。

\begin{figure}[htbp]
  \centering
  \includegraphics[height=5cm]{BA.PNG}
  \caption{双视图下的BA}
  \label{fig:BA}
\end{figure}

通过sfm的方法我们可以获得视图之间的位姿初始值，这个初始值中存在噪声。
在双视图下，我们以其中一个视图对landmark的观测点作为真值，而另一个视图中对相同的landmark的观测点在通过与视图之间的位姿初始值做运算后投影到原视图后可以作为观测值。这样即可构建公式\ref{equ:LSP}的函数。
对该函数优化即可获得视图之间位姿的优化值。

我们可以给出观测方程的具体定义，这里由于涉及相关知识较多，因此直接给出公式。

\begin{equation}
  \label{equ:watch}
  sz_{k,j}=Kexp(\xi^{\wedge})y_{i}
\end{equation}


这里 K为相机内参矩阵，s为scale，z即我们观测到的二维路标点，$exp(\xi^{\wedge})$表示[R t]，
即相机的位姿，它与$x_{k}$等价，$y_{j}$为j时刻的路标点。

由公式\ref{equ:LSP}与公式\ref{equ:watch}此给出优化函数的估计值：

\begin{equation}
  \xi^{*}=arg min\sum_{y_{j}\subseteq j}(z_{k}-z_{k,j}^{\wedge})^{2}
\end{equation}


\subsection{求解方法}

对于非线性方程的求解方法有很多，其中较为常用的有Gaussian-Newtona lgorithm\cite{spedicato1988numerical}、Levenberg–Marquardt algorithm\cite{more1978levenberg}。这两种算法属于数值计算方面，因此不在此展开。

\section{算法框图}